﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace CommTest
{
    public partial class ServoCtrl : Form
    {
        MainForm.SDataStructure lastData;
        private uint BaseAddr = 0x020000;

        public ServoCtrl()
        {
            InitializeComponent();
            comboBoxServoSelection.SelectedIndex = 0; // select servo 1

            pictureBoxImage.Image = new Bitmap(300, 300);
        }

        public void NewData(MainForm.SDataStructure copyStruct)
        {
            lastData = copyStruct;
        }

        private void timerGUI_Tick(object sender, EventArgs e)
        {
            int selected = comboBoxServoSelection.SelectedIndex;

            if (selected == 0)
            {
                textBoxServoState.Text = string.Format("{0}", lastData.State1);
                textBoxServoPosition.Text = string.Format("{0:f1} Pulses", lastData.Position1);
                textBoxServoSpeed.Text = string.Format("{0:f1} rad/s", lastData.Speed1);
                textBoxServoCurrent.Text = string.Format("{0:f1} A", lastData.Current1);
            }
            else
            {
                textBoxServoState.Text = string.Format("{0}", lastData.State2);
                textBoxServoPosition.Text = string.Format("{0:f1} Pulses", lastData.Position2);
                textBoxServoSpeed.Text = string.Format("{0:f1} rad/s", lastData.Speed2);
                textBoxServoCurrent.Text = string.Format("{0:f1} A", lastData.Current2);
            }

            textBoxLoopMS.Text = string.Format("{0:f2} ms", lastData.SRVLoop);
            textBoxLoopMAXMS.Text = string.Format("{0:f2} ms", lastData.SRVLoopMAX);
            textBoxINTLoop.Text = string.Format("{0:f2} us", lastData.SRVINTLoop);
            textBoxINTLoopMAX.Text = string.Format("{0:f2} us", lastData.SRVINTLoopMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", lastData.SRVTotalTime);

            // update status
            int status = 0;
            if (selected == 0) status = lastData.State1;
            else status = lastData.State2;

            float RefMin = -1;
            float RefMax = 1;

            switch (status)
            {
                case 0:
                    pictureOff.BackColor = Color.Lime;
                    pictureDuty.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    pictureStatus.BackColor = Color.Lime;
                    RefMin = -1;
                    RefMax = 1;
                    break;

                case 1:
                    pictureOff.BackColor = Color.Red;
                    pictureDuty.BackColor = Color.Lime;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    pictureStatus.BackColor = Color.Lime;
                    RefMin = -1;
                    RefMax = 1;
                    break;

                case 2:
                    pictureOff.BackColor = Color.Red;
                    pictureDuty.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Lime;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    pictureStatus.BackColor = Color.Lime;
                    RefMin = -1;
                    RefMax = 1;
                    break;

                case 3:
                    pictureOff.BackColor = Color.Red;
                    pictureDuty.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Lime;
                    picturePosition.BackColor = Color.Red;
                    pictureStatus.BackColor = Color.Lime;
                    RefMin = -1000;
                    RefMax = 1000;
                    break;

                case 4:
                    pictureOff.BackColor = Color.Red;
                    pictureDuty.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Lime;
                    pictureStatus.BackColor = Color.Lime;
                    RefMin = 0;
                    RefMax = 4500;
                    break;

                default:
                    pictureOff.BackColor = Color.Red;
                    pictureDuty.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    pictureStatus.BackColor = Color.Red;
                    RefMin = -1;
                    RefMax = 1;
                    break;
            }

            // set min/max
            textBoxMinVal.Text = RefMin.ToString();
            textBoxMaxVal.Text = RefMax.ToString();
            // get ref value
            float refVal = (float)hScrollBarRegVal.Value / (float)hScrollBarRegVal.Maximum;

            // prevent negative Position! (will break nullpos switch)
            if (status == 4)
            {
                if (refVal < 0) refVal = 0;
            }

            refVal *= RefMax;
            textBoxRefVal.Text = refVal.ToString();

            // send ref
            MainForm form = (MainForm)Application.OpenForms[0];
            if (selected == 0) form.SendCANMessage(BaseAddr + 0x0101, refVal);
            if (selected == 1) form.SendCANMessage(BaseAddr + 0x0201, refVal);

            // draw some crap
            Graphics g = Graphics.FromImage(pictureBoxImage.Image);
            g.Clear(Color.Black);

            double Angle = 0; // radians 
            if( selected == 0) Angle = -lastData.Position1/(64*67) * Math.PI*2;
            if( selected == 1) Angle = -lastData.Position2/(64*67) * Math.PI*2;

            float X = (float)Math.Sin(Angle);
            float Y = (float)Math.Cos(Angle);
            Pen p1 = new Pen(Color.White, 5);
            Pen p2 = new Pen(Color.Blue, 3);
            
            g.DrawLine(p1, 150, 150, 150 + X*140, 150 + Y*140);

            if (status == 4) // position mode, draw reference
            {
                double AngleRef = -refVal / (64 * 67) * Math.PI * 2;
                X = (float)Math.Sin(AngleRef);
                Y = (float)Math.Cos(AngleRef);
                g.DrawLine(p2, 150, 150, 150 + X * 100, 150 + Y * 100);
            }

            pictureBoxImage.Invalidate();
        }

        private void buttonModeSelect_Click(object sender, EventArgs e)
        {
            int selected = comboBoxServoSelection.SelectedIndex;
            uint address = BaseAddr + 0x0100;
            if (selected == 1) address = BaseAddr + 0x0200;

            MainForm form = (MainForm)Application.OpenForms[0];
            if (sender == buttonOff) form.SendCANMessage(address, 0);
            else if (sender == buttonDuty) form.SendCANMessage(address, 1);
            else if (sender == buttonTorque) form.SendCANMessage(address, 2);
            else if (sender == buttonSpeed) form.SendCANMessage(address, 3);
            else if (sender == buttonPosition) form.SendCANMessage(address, 4);
        }

        private void textBoxRefVal_Click(object sender, EventArgs e)
        {
            hScrollBarRegVal.Value = 0;
        }
    }
}
